program Test_my_taylor
!use tree_element_MODULE, my_taylor=>taylor, taylor=>taylor
use my_own_da
type(my_taylor) L1,K1,L2,K2,x(2),L0,K0
!real(dp) L1,K1,L2,K2,x(2),L0,K0
!type(normalform) nf
!type(damap) id
call init(3,1,0,0)
call alloc(L1);call alloc(K1);call alloc(L2);call alloc(K2);call alloc(L0);call alloc(K0);
call alloc(x(1));call alloc(x(2));
N=10
L1=0.25D0
K1=20.0D0
L2=0.2D0
K2=-k1
L0=0.1d0
K0=0.D0


X(1)=0.5D0;X(2)=0.D0;

X(1)=X(1) + (1.0_DP.mono.1)
X(2)=X(2) + (1.0_DP.mono.2)

n_tpsa_exp=30

x(2)=tpsa_expt(x(1))
call print( X(2),6)
write(6,*) ' '
x(2)=exp(x(1))
call print( X(2),6)
pause

DO I=1,10
 CALL QUAD_MY_TAYLOR(X,L1,K1,N)
 CALL QUAD_MY_TAYLOR(X,L0,K0,N)
 CALL QUAD_MY_TAYLOR(X,L2,K2,N)
 CALL QUAD_MY_TAYLOR(X,L0,K0,N)
ENDDO

!call alloc(id)
!call alloc(nf)

!id%v(1)=x(1)
!id%v(2)=x(2)
!nf=id

!call print(nf%a_t,6)
!call print(nf%dhdj,6)


WRITE(6,*) " Map for 10 FODO Cells "

WRITE(6,*) " FIRST RAY " 
call print( X(1),6)
WRITE(6,*) " SECOND RAY " 
call print( X(2),6)
end program Test_my_taylor


SUBROUTINE QUAD_MY_TAYLOR(X,L,K,N)
USE my_own_da
!use polymorphic_complextaylor, my_taylor=>taylor, taylor=>taylor
IMPLICIT NONE
INTEGER N,I
TYPE(my_taylor) K,L,X(2),DL
!real(dp) K,L,X(2),DL

call alloc(DL);
DL=L/N

DO I=1,N

X(1)=X(1)+DL*X(2)/SQRT(1.D0-X(2)**2)
X(2)=X(2)-DL*K*X(1)
ENDDO

call kill(DL);

END SUBROUTINE QUAD_MY_TAYLOR


















SUBROUTINE GUN_MY_TAYLOR
USE my_own_da
IMPLICIT NONE
TYPE(my_taylor) Y(2)
REAL(DP) C,FIX(2),X(2)
INTEGER I

x=0.d0
fix=0.d0

do i=1,4
call compute_traj(fix,y)
c=-(y(1)%A(0))/((y(1)%A(2)))
fix(2)=fix(2)+c
x(1)=y(1);x(2)=y(2);
write(6,*) fix
write(6,*) x
enddo

END SUBROUTINE GUN_MY_TAYLOR

subroutine compute_traj(x,y)
USE my_own_da
implicit none
integer i,NGUN
real(dp) x(2),dz,DIS,V0,grav
type(my_taylor)  y(2)

DIS=60.D0
NGUN=10
V0=800.D0
grav=10.d0

dz=dis/ngun

y(1)=x(1) +(1.d0.mono.1)
y(2)=x(2) +(1.d0.mono.2)

do i=1,ngun
 y(1)=y(1)+dz*y(2)/sqrt(v0**2-2.d0*grav*y(1)-y(2)**2)/2.d0
 y(2)=y(2)-dz*grav/sqrt(v0**2-2.d0*grav*y(1)-y(2)**2)
 y(1)=y(1)+dz*y(2)/sqrt(v0**2-2.d0*grav*y(1)-y(2)**2)/2.d0
enddo


end subroutine compute_traj